/**
  ******************************************************************************
  * File Name          : jaka_moveit_driver.cpp
  * Description       : jaka_moveit_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <jaka_driver/jaka_driver.h>


unsigned char step = 0;
unsigned char run = 0;

unsigned char ptcl_cmd = 21;
unsigned char ptcl_idx = 4;   //3:关节位置控制  4:关节抱闸控制
unsigned char ptcl_ste = 0;

float ptcl_tdata[8] = {0, 0, 0, 0, 0, 0, 0, 0};
float ptcl_rdata[8] = {0, 0, 0, 0, 0, 0, 0, 0};

float _act_pos[6] = {0, 0, 0, 0, 0, 0};

/* Private function prototypes -----------------------------------------------*/
/**
  * @brief  接收通信协议反馈数据
  * @param  消息结构体指针
  * @retval 无
  */
void ptclCB(const smart_msgs::ptcl::ConstPtr & msg)
{
  if((msg->idx==3)&&(msg->cmd==3))
  {
    _act_pos[0] = msg->dtx.a1;
    _act_pos[1] = msg->dtx.a2;
    _act_pos[2] = msg->dtx.a3;
    _act_pos[3] = msg->dtx.a4;
    _act_pos[4] = msg->dtx.a5;
    _act_pos[5] = msg->dtx.a6;
  }
}

/**
  * @brief  接收命令控制数据
  * @param  消息结构体指针
  * @retval 无
  */
void goalCB(const jaka_msgs::Goal::ConstPtr & msg)
{
  ptcl_tdata[0] = msg->a1; 
  ptcl_tdata[1] = msg->a2;
  ptcl_tdata[2] = msg->a3;
  ptcl_tdata[3] = msg->a4;
  ptcl_tdata[4] = msg->a5;
  ptcl_tdata[5] = msg->a6;
  net_tcp_write(21, 3, 0, ptcl_tdata);  
}


/**
  * @brief  机械臂驱动主函数
  * @param  消息结构体指针
  * @retval 无
  */
int main(int argc, char** argv)
{
  ros::init(argc, argv, "jaka_driver");
  ros::NodeHandle nh;
  ros::Rate loop_rate(100);

  ros::Publisher pub_states = nh.advertise<sensor_msgs::JointState>("joint_states", 100);
  ros::Subscriber sub_goal = nh.subscribe("/jaka_msgs/goal", 1, goalCB);
  
  smart_msgs::ptcl  order;
  sensor_msgs::JointState joint_state;

  nh.getParam("/jaka_driver/joint_names", joint_state.name);
  nh.getParam("/jaka_driver/joint_position", joint_state.position);

  net_tcp_open();
  
  for(unsigned char i=0; i<JOINT_MAX; i++)
  {
    ptcl_tdata[i] = 128;
    net_tcp_write(21, 4, 0, ptcl_tdata);
    usleep(1000000);
  }

  while(ros::ok())
  {
    if(net_tcp_read(&ptcl_cmd, &ptcl_idx, &ptcl_ste, ptcl_rdata))
    {
      joint_state.header.stamp = ros::Time::now();
      joint_state.position[0] = ptcl_rdata[0];
      joint_state.position[1] = ptcl_rdata[1];
      joint_state.position[2] = ptcl_rdata[2]*-1;
      joint_state.position[3] = ptcl_rdata[3];
      joint_state.position[4] = ptcl_rdata[4];
      joint_state.position[5] = ptcl_rdata[5];
    }
    pub_states.publish(joint_state);

    ros::spinOnce();
    loop_rate.sleep();
  }

  net_tcp_close();
  
  return 0;
}
